#include "matrix.h"
#include <math.h>

#define cos cosf
#define sin sinf

void rotByMatrix44FPU(float m1[4][4], float *vec, float vecOut[4])
{
    float mrot[4][4];

    mrot[0][3] = 0.0f;
    mrot[1][3] = 0.0f;
    mrot[2][3] = 0.0f;

    mrot[3][0] = 0.0f;
    mrot[3][1] = 0.0f;
    mrot[3][2] = 0.0f;

    mrot[3][3] = 1.0f;

    mrot[0][0] = m1[0][0];
    mrot[0][1] = m1[0][1];
    mrot[0][2] = m1[0][2];
    mrot[1][0] = m1[1][0];
    mrot[1][1] = m1[1][1];
    mrot[1][2] = m1[1][2];
    mrot[2][0] = m1[2][0];
    mrot[2][1] = m1[2][1];
    mrot[2][2] = m1[2][2];

    transfByMatrix44FPU(mrot, vec, vecOut);
}

void translateMatrixFPU(float m[4][4], float vec[3], float mout[4][4])
{
    float m2[4][4] = { 
        {1.0f, 0.0f, 0.0f, vec[0]},
        {0.0f, 1.0f, 0.0f, vec[1]},
        {0.0f, 0.0f, 1.0f, vec[2]},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    multMatrix44FPU(m,m2,mout);
}

void rotateMatrixXFPU(float m[4][4], float ang, float mout[4][4], bool r)
{
    float s = sin(ang);
    float c = cos(ang);

    float m2[4][4] = { 
        {1.0f, 0.0f, 0.0f, 0.0f},
        {0.0f,    c,   -s, 0.0f},
        {0.0f,    s,    c, 0.0f},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    if(!r) multMatrix44FPU(m,m2,mout);
    else multMatrix44FPU(m2,m,mout);
}

void rotateMatrixYFPU(float m[4][4], float ang, float mout[4][4], bool r)
{
    float s = sin(ang);
    float c = cos(ang);

    float m2[4][4] = { 
        {   c, 0.0f,    s, 0.0f},
        {0.0f, 1.0f, 0.0f, 0.0f},
        {  -s, 0.0f,    c, 0.0f},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    if(!r) multMatrix44FPU(m,m2,mout);
    else multMatrix44FPU(m2,m,mout);
}

void rotateMatrixZFPU(float m[4][4], float ang, float mout[4][4], bool r)
{
    float s = sin(ang);
    float c = cos(ang);

    float m2[4][4] = { 
        {   c,   -s, 0.0f, 0.0f},
        {   s,    c, 0.0f, 0.0f},
        {0.0f, 0.0f, 1.0f, 0.0f},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    if(!r) multMatrix44FPU(m,m2,mout);
    else multMatrix44FPU(m2,m,mout);
}

void identityMatrix(float m[4][4])
{
    memset(m, 0.0f, sizeof(float)*16);
    m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
}

void fastInverseMatrix(float mIn[4][4], float mOut[4][4])
{
    if(mOut != mIn) memcpy(mOut, mIn, sizeof(float)*16);

    float tmp;

    tmp = mOut[0][1];
    mOut[0][1] = mOut[1][0];
    mOut[1][0] = tmp;

    tmp = mOut[0][2];
    mOut[0][2] = mOut[2][0];
    mOut[2][0] = tmp;

    tmp = mOut[1][2];
    mOut[1][2] = mOut[2][1];
    mOut[2][1] = tmp;

    mOut[0][3] = -mOut[0][3];
    mOut[1][3] = -mOut[1][3];
    mOut[2][3] = -mOut[2][3];
}
